#include "ros/ros.h"
#include "sensor_msgs/PointCloud2.h"
#include <geometry_msgs/Pose2D.h>
 #include <nav_msgs/Odometry.h>
#include <pcl17/ros/conversions.h>
#include <pcl17/point_cloud.h>
#include <pcl17/point_types.h>
#include <pcl17/registration/transforms.h>
#include <tf/tf.h>
#include <pcl17_ros/transforms.h>
#include <pcl17_ros/point_cloud.h>
#include <tf/transform_listener.h>
#include <pcl17/filters/approximate_voxel_grid.h>
#include <pcl17/filters/passthrough.h>
#include <pcl17/filters/radius_outlier_removal.h>
#include <pcl17/registration/ndt.h>

typedef pcl17::PointXYZRGB PointType;
typedef pcl17::PointCloud<PointType> Cloud;
typedef Cloud::ConstPtr CloudConstPtr;
typedef Cloud::Ptr CloudPtr;

class BigCloud {
private:
	std::string inputCloudTopic_;
	std::string outputCloudTopic_;
	std::string odomTopic_;
	
	int rolling_window_;
	int counter;
	double downsample_res_;
	ros::NodeHandle nh_;
	ros::NodeHandle local_nh_;

	ros::Subscriber cloudSub_;
	ros::Subscriber odomSub_;
	
	boost::shared_ptr<ros::Publisher> pc_pub_;

	tf::TransformListener *tf_listener_;
	CloudPtr bigCloud;
	std::vector<CloudPtr> cloud_train;
	bool first_pass;
	double dist_thre_;
	double angle_thre_;
	geometry_msgs::Pose2D prev_pose;
	geometry_msgs::Pose2D curr_pose;

	double ndt_StepSize_;
	double ndt_Resolution_;
	double ndt_TransformationEpsilon_;
	int ndt_MaximumIterations_;

	double rot_angle_thres_;
	int cloud_size_thres_;
	static const int Bad_COV = 100;


public:
	BigCloud(ros::NodeHandle nh, ros::NodeHandle local_nh) :
			local_nh_(local_nh), nh_(nh), bigCloud(new Cloud()) {
		local_nh_.param<std::string>("input_cloud", inputCloudTopic_,
				"elas_ros/point_cloud");
		local_nh_.param<std::string>("output_cloud", outputCloudTopic_,
				"big_cloud");
		local_nh_.param<std::string>("odom_topic", odomTopic_,
						"odom");
		
		local_nh_.param<double>("downsample_res", downsample_res_, 0.05);
		local_nh_.param<double>("dist_thre", dist_thre_, 0.1);
		local_nh_.param<double>("angle_thre", angle_thre_, 5);
		local_nh_.param<int>("rolling_window_size", rolling_window_, 15);

		local_nh_.param<double>("rot_angle_thres", rot_angle_thres_, 10.0);
		local_nh_.param<int>("cloud_size_thres", cloud_size_thres_, 50000);


		// ndt param
		local_nh_.param<int>("ndt_MaximumIterations", ndt_MaximumIterations_,
				35);
		local_nh_.param<double>("ndt_StepSize", ndt_StepSize_, 0.1);
		local_nh_.param<double>("ndt_Resolution", ndt_Resolution_, 1.0);
		local_nh_.param<double>("ndt_TransformationEpsilon",
				ndt_TransformationEpsilon_, 0.01);


		tf_listener_ = new tf::TransformListener(local_nh_, ros::Duration(100));
		cloudSub_ = nh_.subscribe(inputCloudTopic_, 1, &BigCloud::cloud_CB,
				this);
		
		odomSub_ = nh_.subscribe(odomTopic_, 1, &BigCloud::odom_CB,
						this);
		pc_pub_.reset(
				new ros::Publisher(
						nh_.advertise<sensor_msgs::PointCloud2>(
								outputCloudTopic_, 1)));

		cloud_train.resize(rolling_window_);
		counter = 0;
		first_pass = true;
		prev_pose.x = 0.0;
		prev_pose.y = 0.0;
		prev_pose.theta = 0.0;

	}
private:
	void odom_CB(const nav_msgs::Odometry& odometry_msg) {
		
   	double cov = odometry_msg.pose.covariance[0];
		
		if (cov > Bad_COV){
			counter = 0;
			first_pass = true;
		}
        
	}
	
	void cloud_CB(const sensor_msgs::PointCloud2ConstPtr& input) {
		sensor_msgs::PointCloud2 cloud_out;
		tf::StampedTransform sensor_tf;
		try {
            
            tf_listener_->waitForTransform("visual_odometry", "base_link",ros::Time::now(),ros::Duration(3.0));
			pcl17_ros::transformPointCloud("base_link", *input, cloud_out,
					*tf_listener_);
		} catch (tf::TransformException &ex) {
			ROS_ERROR("%s", ex.what());
			return;
		}

		try {
            tf_listener_->waitForTransform("visual_odometry", "base_link",ros::Time::now(),ros::Duration(3.0));
			tf_listener_->lookupTransform("visual_odometry", "base_link",
					input->header.stamp, sensor_tf);
		} catch (tf::TransformException &ex) {
			ROS_ERROR("%s", ex.what());
			return;
		}

		curr_pose.x = sensor_tf.getOrigin().x();
		curr_pose.y = sensor_tf.getOrigin().y();
		curr_pose.theta = tf::getYaw(sensor_tf.getRotation());
		double dist = getDistance(prev_pose,curr_pose);
		double rot_angle = std::abs(angleWrap((curr_pose.theta-prev_pose.theta)*180/3.14));

		
		if ((dist < dist_thre_) && (rot_angle < angle_thre_)){ // don't add clouds if the robot is standing still
			return;
		}
		prev_pose = curr_pose;
		//std::cout<< rot_angle << std::endl;

		CloudPtr tmpPCL(new Cloud());
		pcl17::fromROSMsg(cloud_out, *tmpPCL);
		if(tmpPCL->size()==0){
			return;
		}

		tmpPCL->sensor_origin_[0] = sensor_tf.getOrigin().x();
		tmpPCL->sensor_origin_[1] = sensor_tf.getOrigin().y();
		tmpPCL->sensor_origin_[2] = sensor_tf.getOrigin().z();
		tmpPCL->sensor_orientation_.z() = sensor_tf.getRotation().z();
		tmpPCL->sensor_orientation_.w() = sensor_tf.getRotation().w();
		tmpPCL->sensor_orientation_.x() = sensor_tf.getRotation().x();
		tmpPCL->sensor_orientation_.y() = sensor_tf.getRotation().y();
		passfilterCloud(tmpPCL, tmpPCL, 0, 8, "x");
		repositionCloud(tmpPCL, tmpPCL);
		cloud_train[counter] = tmpPCL;
		bigCloud->sensor_origin_ = cloud_train[counter]->sensor_origin_;
		bigCloud->sensor_orientation_ = cloud_train[counter]->sensor_orientation_;
		bigCloud->header.frame_id = "visual_odometry";
		bigCloud->header.stamp = cloud_train[counter]->header.stamp;
		if (counter < rolling_window_ && first_pass) {
			for (int i = 0; i <= counter; i++) {
				*bigCloud += *cloud_train[i];
			}
		} else {
			for (int i = 0; i < cloud_train.size(); i++) {
				*bigCloud += *cloud_train[i];
			}
		}

		downSampleCloud(bigCloud, bigCloud, downsample_res_);
		bigCloud->width = 1;
		bigCloud->height = bigCloud->points.size();
		pc_pub_->publish(*bigCloud);
		counter++;
		if (counter == rolling_window_) {
			first_pass = false;
			counter = 0;
		}
		bigCloud->clear();

	}



	void repositionCloud(CloudPtr cloud_in, CloudPtr cloud_out) {
		Eigen::Matrix3f rotMatrix =
				cloud_in->sensor_orientation_.toRotationMatrix();
		Eigen::Matrix4f t;
		t(0, 0) = rotMatrix(0, 0);
		t(0, 1) = rotMatrix(0, 1);
		t(0, 2) = rotMatrix(0, 2);
		t(1, 0) = rotMatrix(1, 0);
		t(1, 1) = rotMatrix(1, 1);
		t(1, 2) = rotMatrix(1, 2);
		t(2, 0) = rotMatrix(2, 0);
		t(2, 1) = rotMatrix(2, 1);
		t(2, 2) = rotMatrix(2, 2);

		t(3, 0) = t(3, 1) = t(3, 2) = 0;
		t(3, 3) = 1;
		t(0, 3) = cloud_in->sensor_origin_[0];
		t(1, 3) = cloud_in->sensor_origin_[1];
		t(2, 3) = cloud_in->sensor_origin_[2];
		pcl17::transformPointCloud(*cloud_in, *cloud_out, t);

	}

	void downSampleCloud(CloudPtr input, CloudPtr output, double leafSize) {

		pcl17::ApproximateVoxelGrid<pcl17::PointXYZRGB> sor;
		sor.setInputCloud(input);
		sor.setLeafSize(leafSize, leafSize, leafSize);
		sor.filter(*output);
		//  ROS_INFO("Voxel keypoints: %d",output->points.size());

	}

	void passfilterCloud(CloudPtr cloud_in, CloudPtr cloud_out, float depth_ini,
			float depth_limit, std::string fieldName = "x") {

		pcl17::PassThrough<PointType> passx1;
		passx1.setInputCloud(cloud_in);
		passx1.setFilterFieldName(fieldName);
		passx1.setFilterLimits(depth_ini, depth_limit);
		passx1.filter(*cloud_out);

	}

	void removeOutliers(CloudPtr cloud_in, CloudPtr cloud_out) {

		pcl17::RadiusOutlierRemoval<PointType> radius_outlier_removal;
		radius_outlier_removal.setInputCloud(cloud_in);
		radius_outlier_removal.setRadiusSearch(0.1);
		radius_outlier_removal.setMinNeighborsInRadius(30);
		radius_outlier_removal.filter(*cloud_out);
	}

	double getDistance(geometry_msgs::Pose2D p1, geometry_msgs::Pose2D p2){
			return std::sqrt((p1.x-p2.x)*(p1.x-p2.x)+(p1.y-p2.y)*(p1.y-p2.y));
		}

	double angleWrap(double a){
		if (a > 180){
			a = a - 360;
		}else if(a < -180){
			a = a + 360;
		}
		return a;
	}

	void changeColor(pcl17::PointCloud<pcl17::PointXYZRGB>::Ptr input, int r,
				int g, int b) {
			for (size_t i = 0; i < input->size(); ++i) {
				(*input)[i].r = r;
				(*input)[i].g = g;
				(*input)[i].b = b;
			}
		}

};

main (int argc, char** argv)
{
	ros::init (argc, argv, "big_cloud");
	ros::NodeHandle nh;
	ros::NodeHandle local_nh("~");
	BigCloud bC(nh,local_nh);
	ros::spin();
}

